#include <vector>
#include <Eigen/Core>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <Eigen/Geometry>
#include "Geocentric/LocalCartesian.hpp"
#include <deque>
#include <random>
#include <yaml-cpp/yaml.h>

#define D2R 0.017453292519943295
struct State
{
    Eigen::Vector3d p;
    Eigen::Vector3d v;
    Eigen::Quaterniond q;
};
struct IMUData
{
    double time;
    Eigen::Vector3d acc;
    Eigen::Vector3d gyro;
};
struct PoseData
{
    double time;
    State state;
};
std::deque<IMUData> imu_buff;
std::deque<PoseData> gt_buff;
std::deque<IMUData> current_imu;
PoseData current_gt;
PoseData current_pose;
GeographicLib::LocalCartesian geo_converter(32, 120, 0);
std::ofstream gt_ofs;
std::ofstream pose_ofs;
bool ReadData(const std::vector<std::string> &path)
{
    imu_buff.clear();
    gt_buff.clear();
    std::vector<std::ifstream> reads;
    // int count = 0;
    for (int i = 0; i < path.size(); ++i)
    {
        reads.push_back(std::ifstream(path[i]));
    }
    for (int i = 0; i < path.size(); ++i)
    {
        std::string strs;
        std::getline(reads[i], strs);
    }
    std::string strs;
    while (std::getline(reads[0], strs))
    {
        double time = std::stod(strs);
        std::getline(reads[1], strs);
        std::string temp = "";
        std::vector<double> acc;
        for (int i = 0; i < strs.size(); ++i)
        {
            if (strs[i] == ',')
            {
                acc.push_back(std::stod(temp));
                temp = "";
            }
            else
            {
                temp = temp + strs[i];
            }
        }
        acc.push_back(std::stod(temp));

        std::getline(reads[2], strs);
        temp = "";
        std::vector<double> gyro;
        for (int i = 0; i < strs.size(); ++i)
        {
            if (strs[i] == ',')
            {
                gyro.push_back(std::stod(temp));
                temp = "";
            }
            else
            {
                temp = temp + strs[i];
            }
        }
        gyro.push_back(std::stod(temp));
        IMUData imu;
        imu.time = time;
        imu.acc = Eigen::Vector3d(acc[0], acc[1], acc[2]);
        imu.gyro = Eigen::Vector3d(gyro[0] * D2R, gyro[1] * D2R, gyro[2] * D2R);
        imu_buff.push_back(imu);

        std::getline(reads[3], strs);
        temp = "";
        std::vector<double> ref_pos;
        for (int i = 0; i < strs.size(); ++i)
        {
            if (strs[i] == ',')
            {
                ref_pos.push_back(std::stod(temp));
                temp = "";
            }
            else
            {
                temp = temp + strs[i];
            }
        }
        ref_pos.push_back(std::stod(temp));

        std::getline(reads[4], strs);
        temp = "";
        std::vector<double> ref_vel;
        for (int i = 0; i < strs.size(); ++i)
        {
            if (strs[i] == ',')
            {
                ref_vel.push_back(std::stod(temp));
                temp = "";
            }
            else
            {
                temp = temp + strs[i];
            }
        }
        ref_vel.push_back(std::stod(temp));

        std::getline(reads[5], strs);
        temp = "";
        std::vector<double> ref_att_quat;
        for (int i = 0; i < strs.size(); ++i)
        {
            if (strs[i] == ',')
            {
                ref_att_quat.push_back(std::stod(temp));
                temp = "";
            }
            else
            {
                temp = temp + strs[i];
            }
        }
        ref_att_quat.push_back(std::stod(temp));

        Eigen::Quaterniond q = Eigen::AngleAxisd(90 * D2R, Eigen::Vector3d::UnitZ()) *
                               Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) *
                               Eigen::AngleAxisd(180 * D2R, Eigen::Vector3d::UnitX());
        q = q.inverse();

        PoseData pose;
        pose.time = time;
        double geo_x, geo_y, geo_z;
        geo_converter.Forward(ref_pos[0], ref_pos[1], ref_pos[2], geo_x, geo_y, geo_z);
        pose.state.p = Eigen::Vector3d(geo_x, geo_y, geo_z);
        // pose.state.p = q * Eigen::Vector3d(ref_pos[0], ref_pos[1], ref_pos[2]);
        pose.state.v = q * Eigen::Vector3d(ref_vel[0], ref_vel[1], ref_vel[2]);
        pose.state.q = q * Eigen::Quaterniond(ref_att_quat[0], ref_att_quat[1], ref_att_quat[2], ref_att_quat[3]);
        pose.state.q.normalize();
        gt_buff.push_back(pose);
    }
}

void SavePose(std::ofstream &save_points, PoseData &pose)
{
    save_points.precision(12);
    save_points << pose.time
                << " " << pose.state.p(0)
                << " " << pose.state.p(1)
                << " " << pose.state.p(2)
                << " " << pose.state.q.x()
                << " " << pose.state.q.y()
                << " " << pose.state.q.z()
                << " " << pose.state.q.w()
                << std::endl;
}
bool SaveData()
{
    SavePose(gt_ofs, current_gt);
    SavePose(pose_ofs, current_pose);
}

int main(int argc, char **argv)
{
    int method = std::stoi(argv[1]);
    std::vector<std::string> path;
    std::string prefix("/home/zsp/Desktop/imu/data5/");
    path.push_back(prefix + "time.csv");
    path.push_back(prefix + "accel-0.csv");
    path.push_back(prefix + "gyro-0.csv");
    path.push_back(prefix + "ref_pos.csv");
    path.push_back(prefix + "ref_vel.csv");
    path.push_back(prefix + "ref_att_quat.csv");
    ReadData(path);

    gt_ofs.open(prefix + "gt.txt", std::fstream::out);
    pose_ofs.open(prefix + "pose.txt", std::fstream::out);

    Eigen::Vector3d G_ = Eigen::Vector3d(0, 0, 9.79484197226504);
    Eigen::Vector3d angular_gyro_bias_(0, 0, 0);
    Eigen::Vector3d linear_acc_bias_(0, 0, 0);
    current_gt = gt_buff.front();
    current_pose = current_gt;
    for (int i = 1; i < imu_buff.size(); ++i)
    {
        current_gt = gt_buff[i];
        current_pose.time = imu_buff[i].time;
        double delta_t = imu_buff[i].time - imu_buff[i - 1].time;
        Eigen::Vector3d angular_vel_curr = imu_buff[i].gyro - angular_gyro_bias_;
        Eigen::Vector3d angular_vel_prev = imu_buff[i - 1].gyro - angular_gyro_bias_;
        Eigen::Vector3d angular_delta;
        if (method == 0)
        {
            angular_delta = delta_t*angular_vel_prev;
        }
        else
        {
            angular_delta = 0.5 * delta_t * (angular_vel_curr + angular_vel_prev);
        }
        
        // update orientation:
        double angular_delta_mag = angular_delta.norm();
        // direction:
        Eigen::Vector3d angular_delta_dir = angular_delta.normalized();
        // build delta q:
        double angular_delta_cos = cos(angular_delta_mag / 2.0);
        double angular_delta_sin = sin(angular_delta_mag / 2.0);
        Eigen::Quaterniond dq(
            angular_delta_cos,
            angular_delta_sin * angular_delta_dir.x(),
            angular_delta_sin * angular_delta_dir.y(),
            angular_delta_sin * angular_delta_dir.z());
        Eigen::Quaterniond q = current_pose.state.q * dq;
        q.normalize();
        // write back:
        Eigen::Matrix3d R_prev = current_pose.state.q.toRotationMatrix();
        current_pose.state.q = q;
        Eigen::Matrix3d R_curr = current_pose.state.q.toRotationMatrix();
        // get velocity delta:
        Eigen::Vector3d linear_acc_curr = R_curr * (imu_buff[i].acc - linear_acc_bias_) - G_;
        Eigen::Vector3d linear_acc_prev = R_prev * (imu_buff[i - 1].acc - linear_acc_bias_) - G_;
        Eigen::Vector3d velocity_delta = 0.5 * delta_t * (linear_acc_curr + linear_acc_prev);
        // update position:
        current_pose.state.p += delta_t * current_pose.state.v + 0.5 * delta_t * velocity_delta;
        current_pose.state.v += velocity_delta;
        SaveData();
    }
    return 0;
}
